#include "katana_utils.h"
#include <sys/time.h>
#include <iostream>
#include <pthread.h>
#include "Matrix.h"

// in encoder increments per half second ..  
#define MAX_SPEED 2000

SplineMovement::SplineMovement(Matrix& in,CLMBase * katana)
{
  /* in_step_time : time between 2 samples in the "in" Matrix in ms
   */
  int in_steps = in.RowSize();
  int in_totaltime = (int) in(in_steps-1,0); //  milliseconds
  int in_step_time = in_totaltime/in_steps;
  this->nMotors = in.ColumnSize()-1;
  this->time_per_step = 50;  // this means 500 ms per spline step
  int steps = (int)((float) (in_steps*in_step_time/(time_per_step*10)));
  if (steps<20)
    steps=20;
  parameters = new short** [steps];
  for (int i = 0; i < steps; i++)
    parameters[i] = new short* [nMotors];
  for (int i = 0; i < steps; i++)
    for (int j=0;j<nMotors;j++)
      parameters[i][j] = new short[7];
  this->steps = steps;
  this->initialPosition = new short[nMotors];
  this->finished = 1;
  this->katana = katana;
  std::cout << " Movement initialised :" << std::endl;
  std::cout << in_totaltime << " total tine (ms)" << std::endl;
  std::cout << nMotors << " Motors" << std::endl;
  std::cout << steps << " Steps" << std::endl;
  GetFromMatrix(in); 
}


void SplineMovement::GetFromMatrix(Matrix& in)
{
 
  mot = katana->GetBase()->GetMOT();
  int rawsteps=in.RowSize();
  double* timearray = new double [steps+1];
  double** dataarray = new double* [steps+1];
  int * selectedIdx = new int[steps+1];
  int tstep = 0;
  for (int i = 0; i < steps+1; i++){
    dataarray[i] = new double[nMotors];
    // finding the nearest time
    while((in(tstep,0) < (double)i*time_per_step*10)&&(tstep < rawsteps-1))
      tstep++;
    timearray[i] = in(tstep,0)/10.0; 
    selectedIdx[i]=tstep;
  }
  
  for (int i=0;i<nMotors;i++) // Converting radian to encoders values 
    {
      joint.epc         = mot->arr[i].GetInitialParameters()->encodersPerCycle;
      joint.encOffset   = mot->arr[i].GetInitialParameters()->encoderOffset;
      joint.angleOffset = mot->arr[i].GetInitialParameters()->angleOffset;
      joint.angleStop   = mot->arr[i].GetInitialParameters()->angleStop;
      joint.rotDir      = mot->arr[i].GetInitialParameters()->rotationDirection;
      for (int n=0;n<steps+1;n++){
	 int encVal = KNI_MHF::rad2enc((double)in(selectedIdx[n],i+1), 
				       joint.angleOffset, 
				       joint.epc, 
				       joint.encOffset, 
				       joint.rotDir);
	 if((mot->arr[i].GetEncoderMinPos()>encVal) ||
	    (mot->arr[i].GetEncoderMaxPos()<encVal))  // prevents forbidden values 
	   {
	     std::cout << "impossible movement .. " << std::endl;
	     std::cout << "on motor "<< i<<" at time step " << selectedIdx[n] << std::endl; 
	     if (mot->arr[i].GetEncoderMinPos()>encVal)
	       encVal = mot->arr[i].GetEncoderMinPos();
	     else 
	       encVal = mot->arr[i].GetEncoderMaxPos();
	   }
	 if(n>0)
	   if( fabs(dataarray[n-1][i]-encVal) > MAX_SPEED )
	     {
	       std::cout << " too fast gesture" << std::endl;
	       std::cout << "on motor "<< i<<" at time step " << n << std::endl; 
	       //return;
	     }
	 dataarray[n][i] = encVal;				   
      }
    }
  
  // calculate spline

  double* encoderarray = new double [steps+1];
  double* arr_p1 = new double [steps];
  double* arr_p2 = new double [steps];
  double* arr_p3 = new double [steps];
  double* arr_p4 = new double [steps];
  double s_time;
  

  for (int i = 0; i < nMotors; i++) 
    {
      for(int j=0;j<2;j++)  // prevents from starting movement with a too big acceleration..
	encoderarray[j] = dataarray[0][i];
      encoderarray[2] = dataarray[1][i];

    for (int j = 3; j < steps+1; j++) 
      {
      encoderarray[j] = dataarray[j][i];
      }
    katana->splineCoefficients(steps, timearray, encoderarray, arr_p1, arr_p2, arr_p3, arr_p4);
    //store parameters for G command to motor i

    for (int j = 0; j < steps; j++) {
      //motor number
      parameters[j][i][0] = (short) i;
      //targetencoder
      parameters[j][i][1] = (short) encoderarray[j + 1];
      // robot time (in 10ms steps)
      s_time = (timearray[j + 1] - timearray[j]);
      parameters[j][i][2] = (short) s_time;
      // the four spline coefficients
      // the actual position, round
      parameters[j][i][3] = (short) floor(arr_p1[j] + 0.5);
      // shift to be firmware compatible and round
      parameters[j][i][4] = (short) floor(64 * arr_p2[j] / s_time + 0.5);
      parameters[j][i][5] = (short) floor(1024 * arr_p3[j] / pow(s_time, 2) + 0.5);
      parameters[j][i][6] = (short) floor(32768 * arr_p4[j] / pow(s_time, 3) + 0.5);
    }
    this->initialPosition[i]= parameters[0][i][3]; // get initial parameters
  }
   
  delete timearray;
  for (int i = 0; i < (steps); i++)
    delete dataarray[i];
  delete dataarray;
  delete encoderarray;
  delete arr_p1;
  delete arr_p2;
  delete arr_p3;
  delete arr_p4;
  
}



void SplineMovement::initMovement()
{
  char c=0;
  currentstep = 0;
  finished = 0;
 //Set speed, acceleration and crash limits
  katana->setRobotVelocityLimit(40);
  katana->setRobotAccelerationLimit(1);

  mot = katana->GetBase()->GetMOT();
  mot->arr[0].setCrashLimit(200);
  for (int j=1;j<nMotors;j++){
    mot->arr[j].setCrashLimit(500);
  }
  katana->enableCrashLimits();
  std::cout << "Crash limits are set on" << std::endl;
  feedbackValues = new int*[500];
  valCount = 0;
  katana->switchRobotOn();  


  std::cout << "Warning : press space to go to initial position" <<std::endl;
  c=0;
  while (c!=' ')
    c=getch();

  katana->switchRobotOn();
  for (int j=0;j<nMotors;j++)
    katana->moveMotorToEnc(j,(int)initialPosition[j],1); // now blocks until target is reached
}

void SplineMovement::startMovement_threaded()
{
  pthread_create(&mThread,NULL,runThread,(void *) this); // starting movement thread
  
}

void SplineMovement::endMovement_threaded()
{
  pthread_join(mThread,NULL);

  //katana->switchRobotOff();
  feedbackMatrix = encodersToMatrix(katana,feedbackValues,valCount);
}

void SplineMovement::freeze()
{
  if(!this->finished)
    {
      std::cout << "freezing movement.." << std::endl;
      steps = currentstep + 1;
      // this->katana->freezeRobot();
      this->endMovement_threaded();
      // purges spline buffer in the robot : 
      
    }
}  

void SplineMovement::playMovement()
{
  while(!finished)
    {
      sendSplineStep();
    }
}

int SplineMovement::sendSplineStep()
{
  struct timezone tz;
  struct timeval current_time;
  long int elapsed=0;
  int elapsed_sec=0;
  //  int is_blocked = 0;
  if(finished==0) {
    /*for(int i=0;i<nMotors;i++)
      {
	if(mot->arr[i].GetBlocked())
	  is_blocked=1;
      }
    if(is_blocked>0)
      {
	// blocked motors
	std::cout << "== seems that motors are blocked .. " << std::endl;
        for(int j=0;j<nMotors;j++) 
	  {
	    short * p = parameters[currentstep][j];
	    std::cout << p[0] <<" " << p[1] <<" " <<  p[2] <<" " << p[3] <<std::endl;
	  }
	finished = 1;
	return -1;
	}*/
    if(currentstep==0) // first call 
      {
	gettimeofday(&begin_time,&tz);
      }
    else 
      {
	gettimeofday(&current_time,&tz); 
	elapsed = current_time.tv_usec  - begin_time.tv_usec;
	elapsed_sec = current_time.tv_sec - begin_time.tv_sec;
	if (elapsed < 0) 
	  {
	    elapsed =(elapsed_sec-1) * 1000000 - elapsed;
	  }
	else 
	  {
	    elapsed = elapsed_sec * 1000000 + elapsed;
	  }
	
	if(elapsed >= (time_per_step*(steps)*10000))
	  {
	    finished = 1; // end of Movement 
	  }
      }
    feedbackValues[valCount] = new int[7]; // grab encoders data 
    getEncodersValues(katana,feedbackValues[valCount],begin_time);
    valCount++;
    if(currentstep < steps)
      {
	for(int j=0;j<nMotors;j++) 
	  {
	    short * p = parameters[currentstep][j];
	    katana->sendSplineToMotor((unsigned short) p[0], p[1], p[2],
				      p[3], p[4], p[5], p[6]);
	  }
	if(currentstep == steps-1)
	  {
	    katana->startSplineMovement(2, 1);
	  }
	else
	  {
	    katana->startSplineMovement(2, 0);
	  }
	currentstep += 1;
      }
    return 1;
  }
  return -1;
}

void *runThread(void *_tgtMvt) {
  SplineMovement *tgtMvt = (SplineMovement *)_tgtMvt;
  tgtMvt->playMovement();
  printf("Deleting object\n");
  // delete tgtMvt;
  pthread_exit(NULL);
}


